%% Parameters Identification
% This script identifies chosen parameters based on provided TbE and J
% matrices. If you use your own manipulator, please obtain these matrices
% first in "symbolic_transform.m" script

% Choose 1 of 3 datasets
%% Dataset1: simulation B with camera
clear all;
clc;
load('data_simCam.mat');
measurements = table2array(data_simCam);
j = 251;

%% Dataset2: experiment A
clear all;
clc;
load('data_expCam1.mat');
load('data_expCam2.mat');
load('data_expCam3.mat');
measurements = table2array(data_cam1);
measurements = [measurements; table2array(data_cam2)];
measurements = [measurements; table2array(data_cam3)];

% counter for number of measured of points)
j=7500;

%% Dataset2: experiment B
clear all;
clc;
load('data_expB.mat');
measurements = table2array(cam2i0);
j = 1250;

%% Identification part
%counter for vectors
jv = 3*j;

% q - joint values from measurements
q = measurements(1:j, 21:26);

% matrix of xyz coodrinates of a TCP (camera sensor values)
Yc = measurements(1:j, 2:4);
Yc = 1000*Yc;

% default offset of the joints
o = [0, pi, 0, 0, 0, pi];
o = transpose(o);
o1 = o(1,1); o2 = o(2,1); o3 = o(3,1); o4 = o(4,1); o5 = o(5,1); o6 = o(6,1);

% first guess - more accurate values are better, but it should coverge
% anyway
X = [431 555 -460 1 0 0 0 1 0 0 0 1 0 0 66];
X = transpose(X);

x0 = X(1,1); y0 = X(2,1); z0 = X(3,1); xE = X(13,1); yE = X(14,1); zE = X(15,1);
r11 = X(4,1); r21 = X(5,1); r31 = X(6,1);
r12 = X(7,1); r22 = X(8,1); r32 = X(9,1);
r13 = X(10,1); r23 = X(11,1); r33 = X(12,1);

% iteration cycle
for iteration = 1:100
    
    % calculate transformation Ym for every position
    for m = 1:j 
        q1 = q(m,1); q2 = q(m,2); q3 = q(m,3); q4 = q(m,4); q5 = q(m,5); q6 = q(m,6);
        
        % transformation matrix from symbolic_transform.m
        TbE = [ - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))), - cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))), (1519*r13)/10 + x0 - (1707*sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))))/20 + (853*cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))/4 - (853*sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))/4 + (4873*cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)))/20 - (819*cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)))/10 - (819*sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))))/10 - (552*r12*cos(o1 + q1))/5 + (552*r11*sin(o1 + q1))/5 + (4873*r13*sin(o2 + q2))/20 - xE*(cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) + sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))))) + yE*(sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))))/20 - zE*(cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)) + sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))));- cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))), - cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))), (1519*r23)/10 + y0 - (1707*sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))))/20 + (853*cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))/4 - (853*sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))/4 + (4873*cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)))/20 - (819*cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)))/10 - (819*sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))))/10 - (552*r22*cos(o1 + q1))/5 + (552*r21*sin(o1 + q1))/5 + (4873*r23*sin(o2 + q2))/20 - xE*(cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) + sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))))) + yE*(sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))))/20 - zE*(cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)) + sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))))); - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))), - cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))), (1519*r33)/10 + z0 - (1707*sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))))/20 + (853*cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))/4 - (853*sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))/4 + (4873*cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)))/20 - (819*cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)))/10 - (819*sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))))/10 - (552*r32*cos(o1 + q1))/5 + (552*r31*sin(o1 + q1))/5 + (4873*r33*sin(o2 + q2))/20 - xE*(cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) + sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))))) + yE*(sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))))/20 - zE*(cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)) + sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))));                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   0,                                                                                                                                                                                                                                                                                                                                                                                                                                                             0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         1];
         
        Ym(m,1) = TbE(1,4);
        Ym(m,2) = TbE(2,4);
        Ym(m,3) = TbE(3,4);
    end
    
    % difference between measured position Yc and actual postion Ym
    Yd = Yc(1:j,:) - Ym;
    
    % transforming the calculated position matrix Yd into vector dY
    for m = 1:j
        dY(((m-1)*3+1):((m-1)*3+3),:) = transpose(Yd(m,:));
    end
    
    % calculating Jacobian matrix for every position, Jacobian obtained from "symbolic_transform.m" script
    for m = 1:j
        q1 = q(m,1); q2 = q(m,2); q3 = q(m,3); q4 = q(m,4); q5 = q(m,5); q6 = q(m,6);
        
        J(((m-1)*3+1):((m-1)*3+3),:) = [ 1, 0, 0, (552*sin(o1 + q1))/5 - (1707*cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)))/20 - (1707*sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)))/20 + zE*(sin(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) + cos(o5 + q5)*sin(o1 + q1)) - xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) - cos(o6 + q6)*(cos(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) - sin(o1 + q1)*sin(o5 + q5))) - yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) + sin(o6 + q6)*(cos(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) - sin(o1 + q1)*sin(o5 + q5))) + (4873*cos(o1 + q1)*cos(o2 + q2))/20 + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))))/10 + (819*cos(o5 + q5)*sin(o1 + q1))/10 + (853*cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3))/4 - (853*cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))/4,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0, (4873*cos(o2 + q2)*sin(o1 + q1))/20 - xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) - cos(o6 + q6)*(cos(o1 + q1)*sin(o5 + q5) + cos(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))) - yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) + sin(o6 + q6)*(cos(o1 + q1)*sin(o5 + q5) + cos(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)))/20 - (1707*sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)))/20 - (819*cos(o1 + q1)*cos(o5 + q5))/10 - (552*cos(o1 + q1))/5 - zE*(cos(o1 + q1)*cos(o5 + q5) - sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)))) + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))/10 + (853*cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1))/4 - (853*sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))/4,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0, (4873*sin(o2 + q2))/20 + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3))))/10 + (853*cos(o2 + q2)*sin(o3 + q3))/4 + (853*cos(o3 + q3)*sin(o2 + q2))/4 + (1707*cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))/20 - (1707*sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)))/20 + xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2))) + cos(o5 + q5)*cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))) + yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2))) - cos(o5 + q5)*sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))) + zE*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3))) + 1519/10,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0, - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))), - cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))));0, 1, 0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0, (552*sin(o1 + q1))/5 - (1707*cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)))/20 - (1707*sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)))/20 + zE*(sin(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) + cos(o5 + q5)*sin(o1 + q1)) - xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) - cos(o6 + q6)*(cos(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) - sin(o1 + q1)*sin(o5 + q5))) - yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) + sin(o6 + q6)*(cos(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) - sin(o1 + q1)*sin(o5 + q5))) + (4873*cos(o1 + q1)*cos(o2 + q2))/20 + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))))/10 + (819*cos(o5 + q5)*sin(o1 + q1))/10 + (853*cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3))/4 - (853*cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))/4,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0, (4873*cos(o2 + q2)*sin(o1 + q1))/20 - xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) - cos(o6 + q6)*(cos(o1 + q1)*sin(o5 + q5) + cos(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))) - yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) + sin(o6 + q6)*(cos(o1 + q1)*sin(o5 + q5) + cos(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)))/20 - (1707*sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)))/20 - (819*cos(o1 + q1)*cos(o5 + q5))/10 - (552*cos(o1 + q1))/5 - zE*(cos(o1 + q1)*cos(o5 + q5) - sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)))) + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))/10 + (853*cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1))/4 - (853*sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))/4,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0, (4873*sin(o2 + q2))/20 + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3))))/10 + (853*cos(o2 + q2)*sin(o3 + q3))/4 + (853*cos(o3 + q3)*sin(o2 + q2))/4 + (1707*cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))/20 - (1707*sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)))/20 + xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2))) + cos(o5 + q5)*cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))) + yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2))) - cos(o5 + q5)*sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))) + zE*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3))) + 1519/10,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0, - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))), - cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))));0, 0, 1,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0, (552*sin(o1 + q1))/5 - (1707*cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)))/20 - (1707*sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)))/20 + zE*(sin(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) + cos(o5 + q5)*sin(o1 + q1)) - xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) - cos(o6 + q6)*(cos(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) - sin(o1 + q1)*sin(o5 + q5))) - yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) + sin(o6 + q6)*(cos(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))) - sin(o1 + q1)*sin(o5 + q5))) + (4873*cos(o1 + q1)*cos(o2 + q2))/20 + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3) - cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o1 + q1)*cos(o2 + q2)*sin(o3 + q3) + cos(o1 + q1)*cos(o3 + q3)*sin(o2 + q2))))/10 + (819*cos(o5 + q5)*sin(o1 + q1))/10 + (853*cos(o1 + q1)*cos(o2 + q2)*cos(o3 + q3))/4 - (853*cos(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))/4,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0, (4873*cos(o2 + q2)*sin(o1 + q1))/20 - xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) - cos(o6 + q6)*(cos(o1 + q1)*sin(o5 + q5) + cos(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))) - yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))) + sin(o6 + q6)*(cos(o1 + q1)*sin(o5 + q5) + cos(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)))/20 - (1707*sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)))/20 - (819*cos(o1 + q1)*cos(o5 + q5))/10 - (552*cos(o1 + q1))/5 - zE*(cos(o1 + q1)*cos(o5 + q5) - sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2)))) + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1) - sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o1 + q1)*sin(o3 + q3) + cos(o3 + q3)*sin(o1 + q1)*sin(o2 + q2))))/10 + (853*cos(o2 + q2)*cos(o3 + q3)*sin(o1 + q1))/4 - (853*sin(o1 + q1)*sin(o2 + q2)*sin(o3 + q3))/4,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0, (4873*sin(o2 + q2))/20 + (819*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3))))/10 + (853*cos(o2 + q2)*sin(o3 + q3))/4 + (853*cos(o3 + q3)*sin(o2 + q2))/4 + (1707*cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))/20 - (1707*sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)))/20 + xE*(sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2))) + cos(o5 + q5)*cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))) + yE*(cos(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)) - sin(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2))) - cos(o5 + q5)*sin(o6 + q6)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3)))) + zE*sin(o5 + q5)*(cos(o4 + q4)*(cos(o2 + q2)*sin(o3 + q3) + cos(o3 + q3)*sin(o2 + q2)) + sin(o4 + q4)*(cos(o2 + q2)*cos(o3 + q3) - sin(o2 + q2)*sin(o3 + q3))) + 1519/10, - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))), - cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))))];
         
    end
    
    Jt = transpose(J);
    
    % calculating error
    p = inv(Jt*J)*Jt*dY;
    
    % calculating new X
    X = X + p;
    
    % changing values for the next iteration
    x0 = X(1,1); y0 = X(2,1); z0 = X(3,1); xE = X(13,1); yE = X(14,1); zE = X(15,1);
    r11 = X(4,1); r21 = X(5,1); r31 = X(6,1);
    r12 = X(7,1); r22 = X(8,1); r32 = X(9,1);
    r13 = X(10,1); r23 = X(11,1); r33 = X(12,1);
    
    % if convergated, break the cycle
    convergence = abs(p);
    if convergence < 0.000001
        iteration;
        break;
    end
    
    % if not convergated in 100 iterations, send error message
    if iteration == 100
        %msgbox('NOT Convergated');
    end
end

% show results
X

%% Validating results - plotting

p = ones(j,3);

% calculate position of the end effector for every measured point
for m = 1:j 
    q1 = q(m,1); q2 = q(m,2); q3 = q(m,3); q4 = q(m,4); q5 = q(m,5); q6 = q(m,6);
    
    % TbE determined in "Symbolic_transform.m"
    TbE = [ - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))), - cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))), (1519*r13)/10 + x0 - (1707*sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))))/20 + (853*cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)))/4 - (853*sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))/4 + (4873*cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)))/20 - (819*cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)))/10 - (819*sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))))/10 - (552*r12*cos(o1 + q1))/5 + (552*r11*sin(o1 + q1))/5 + (4873*r13*sin(o2 + q2))/20 - xE*(cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) + sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))))) + yE*(sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))) - sin(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))))/20 - zE*(cos(o5 + q5)*(r12*cos(o1 + q1) - r11*sin(o1 + q1)) + sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) + r13*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r11*cos(o1 + q1) + r12*sin(o1 + q1)) - r13*cos(o2 + q2)))));- cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))), - cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))), (1519*r23)/10 + y0 - (1707*sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))))/20 + (853*cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)))/4 - (853*sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))/4 + (4873*cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)))/20 - (819*cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)))/10 - (819*sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))))/10 - (552*r22*cos(o1 + q1))/5 + (552*r21*sin(o1 + q1))/5 + (4873*r23*sin(o2 + q2))/20 - xE*(cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) + sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))))) + yE*(sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)))) - sin(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))))/20 - zE*(cos(o5 + q5)*(r22*cos(o1 + q1) - r21*sin(o1 + q1)) + sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) + r23*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r21*cos(o1 + q1) + r22*sin(o1 + q1)) - r23*cos(o2 + q2))))); - cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))), sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))), - cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)) - sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))), (1519*r33)/10 + z0 - (1707*sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))))/20 + (853*cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)))/4 - (853*sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))/4 + (4873*cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)))/20 - (819*cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)))/10 - (819*sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))))/10 - (552*r32*cos(o1 + q1))/5 + (552*r31*sin(o1 + q1))/5 + (4873*r33*sin(o2 + q2))/20 - xE*(cos(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) + sin(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))))) + yE*(sin(o6 + q6)*(cos(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))) - sin(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1))) - cos(o6 + q6)*(sin(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2))) + cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))))) - (1707*cos(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))))/20 - zE*(cos(o5 + q5)*(r32*cos(o1 + q1) - r31*sin(o1 + q1)) + sin(o5 + q5)*(sin(o4 + q4)*(cos(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)) + sin(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2))) - cos(o4 + q4)*(cos(o3 + q3)*(cos(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) + r33*sin(o2 + q2)) - sin(o3 + q3)*(sin(o2 + q2)*(r31*cos(o1 + q1) + r32*sin(o1 + q1)) - r33*cos(o2 + q2)))));                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   0,                                                                                                                                                                                                                                                                                                                                                                                                                                                             0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         1];
        
    % array of position vectors
    p(m,1) = TbE(1,4);
    p(m,2) = TbE(2,4);
    p(m,3) = TbE(3,4);
end

figure(1)

% points of origin
scatter3(Yd(:,1),Yd(:,2),Yd(:,3), 'r')
hold on

% calculated robot's base
scatter3(X(1,1),X(2,1),X(3,1), 'x', 'b')

% calculated points
plot3(p(1:j,1),p(1:j,2),p(1:j,3), 'k', 'LineWidth', 2);

xlabel('X [mm]'); ylabel('Y [mm]'); zlabel('Z [mm]');
grid on;

% measured points 
if j == 251 || j == 1250
    plot3(Yc(1:j,1),Yc(1:j,2),Yc(1:j,3), 'm--', 'LineWidth', 3)
    
    legend('Points of origin - "b" frames', 'Determined base of robot - "0" frame', 'Robot path', 'Measured path');
else
    interval = int16(j/3);
    
    plot3(Yc(1:interval,1),Yc(1:interval,2),Yc(1:interval,3), 'g')    
    plot3(Yc(interval:2*interval,1),Yc(interval:2*interval,2),Yc(interval:2*interval,3), 'm')
    plot3(Yc(2*interval:j,1),Yc(2*interval:j,2),Yc(2*interval:j,3), 'r')
    
    legend('Points of origin - "b" frames', 'Determined base of robot - "0" frame', 'Robot path', 'Camera 1','Camera 2','Camera 3');
end
hold off